#include "WPILib.h" 
#include "PandroidSettings.h"
#include "MotorWrapper.h"
#include "ConveyorMaster.h"
#include "DriverConsole.h"
#include "Math.h"

class PanShoot {
public: 
	void Update();
	static PanShoot* GetInstance(NetworkTable* smartDashboard, DriverConsole* myDrive);
	void Autonomous();
	void Teleop();
	void PrintDump();

private:
	PanShoot(NetworkTable* smartDashboard, DriverConsole* myDrive);
	~PanShoot();
	bool  CheckHAngle();
	bool  CheckVAngle(); 
	bool  CheckSpeed(); 
	
	void  Search();
	void  SetSpeed(float speed); 
	void  SetHAngle(float angle);
	void  SetVAngle(float angle);
	float getVAngle();
	void Rotate180();
	float GetStretchVoltage(float angle);
	void GoToAngle(float angle);
	bool ZeroPitch();
	void MovePitch(float input);
	
	static PanShoot* _instance;
	
	Jaguar *_rotatingTurretMotor; 
	MotorWrapper *_shootTurretMotorTop;
	MotorWrapper *_shootTurretMotorBot;
	MotorWrapper *_pitchTurretMotor;
	Encoder *_turretEncoder;
	PIDController *_turretPID;
	PIDController *_pitchPID;
		
	NetworkTable *_networkTable;
	
	AnalogChannel* _angleSensor;
	
	ConveyorMaster* _conveyor;
	
	DriverConsole* _driveConsole;
	NetworkTable *_smartDashboard;
	
	bool _isReady;
	bool _targetFound;
	bool _startSearch;
	bool _lastTargetAngle;
	bool _lastRotBut;
	int _angleNumClicks;
	bool _canClick;
};
	
//Horizontal Angle: 0 degrees is forward, -180 is full left, 180 is full right
//Vertical Angle: 0 is minimum elevation, 90 is theoretical maximum elevation
//Speed is in revolutions per minute
	
